/*
 * accel functions
 *
 */
#include "config.h"
#include "ebike-accel.h"
#include "ebike-tools.h"
#include <WProgram.h>
#include <math.h>






// 3axes values to correct the Accel values to G
static float offsetx=1.5707963,offsety=1.5707963,offsetz=0.0;
static int first=1;


#ifndef CONFIG_WITH_ACCEL_I2C


//
//get G from Volt
//
double VToG(float volt){
  //double sens=0.8; //1,5G MMA7260Q
  return (volt)/0.8;
}

//
//get volt from ADC
//
float adcToV(double adc){
  return (float)adc*vbit;
}


void accel_init(){
}


int accel_update(double v[],double v1[],double n[], double a[], double vref, double mass){
  //make accelerometer happy
  v[X]=adcToV(fanalogRead(P_ACCX,2)+adjx)-vzero;
  v[Y]=adcToV(fanalogRead(P_ACCY,2)+adjy)-vzero;
  v[Z]=adcToV(fanalogRead(P_ACCZ,2)+adjz)-vzero;
  double r=module((v[X]),(v[Y]),(v[Z]));
  
  // rotate XYZ to fit G
  a[X]=alpha((v[X]),r)+offsetx;
  a[Y]=alpha((v[Y]),r)+offsety;
  a[Z]=alpha((v[Z]),r)+offsetz;

  // get the projected vectors[x,y,z] after rotation
  v1[X]=aalpha(a[X]+orthogonal,r);
  v1[Y]=aalpha(a[Y]+orthogonal,r);
  v1[Z]=aalpha(a[Z],r);
  
  // get Force in newton F[N]=m*accel
  n[X]=VToG(v1[X])*9.81*mass;
  n[Y]=VToG(v1[Y])*9.81*mass;
  n[Z]=VToG(v1[Z])*9.81*mass;
  
  //rotate to the initial position
  if (first){
    offsetx=offsetx-a[X];//(dzero+adjx-fADC[1]);
    offsety=offsety-a[Y];//(dzero+adjy-fADC[2]);
    offsetz=offsetz-a[Z];//(dzero+adjz-fADC[3]);
    first=0;
    return 0;
  }
  
  
  return 1;
}
#else
#include "Adxl345.h"
Accelerometer adxl;

void accel_init(){
  // init adxl to 2g
  adxl.powerOn();
  adxl.setRangeSetting(2);
}

int accel_update(double v[],double v1[],double n[], double a[], double vref, double mass){

  //read accelerometer 
  adxl.readG(&v[X],&v[Y],&v[Z]);  
  double r=module((v[X]),(v[Y]),(v[Z]));
  
  // rotate XYZ to fit G
  a[X]=alpha((v[X]),r)+offsetx;
  a[Y]=alpha((v[Y]),r)+offsety;
  a[Z]=alpha((v[Z]),r)+offsetz;

  // get the projected vectors[x,y,z] after rotation
  v1[X]=aalpha(a[X]+orthogonal,r);
  v1[Y]=aalpha(a[Y]+orthogonal,r);
  v1[Z]=aalpha(a[Z],r);
  
  // get Force in newton F[N]=m*accel
  n[X]=(v1[X])*9.81*mass;
  n[Y]=(v1[Y])*9.81*mass;
  n[Z]=(v1[Z])*9.81*mass;
  
  //rotate to the initial position
  if (first){
    offsetx=offsetx-a[X];//(dzero+adjx-fADC[1]);
    offsety=offsety-a[Y];//(dzero+adjy-fADC[2]);
    offsetz=offsetz-a[Z];//(dzero+adjz-fADC[3]);
    first=0;
    return 0;
  }
  
  
  return 1;
}

#endif

